from hexapod import RealHexapod
from time import time, sleep
from config import *
from movement import MovementMode

REACT_DELAY = movement_interval * 0.001

# 当前模式
cur_mode = MovementMode.MOVEMENT_STANDBY.value

def setCurrentMode(mode):
    global cur_mode
    cur_mode = mode

def getCurrentMode():
    return cur_mode

# 处理运动子线程
def web_moving_thread(mode_changed, servo_config_changed):
    # Hexapod instance
    pi_hexa = RealHexapod()
    pi_hexa.init()

    # standby 循环次数
    standby_loop_count = 500
    loop_count = 0

    while True:
        if servo_config_changed.is_set():
            servo_config_changed.clear()
            pi_hexa.set_servo_offset(ServoConfig.getOffset())

        if mode_changed.is_set():
            t0 = time()
            #print(f"run mode: {cur_mode}, t:{t0}")
            pi_hexa.process_movement(cur_mode)
            time_spent = time() - t0
            if time_spent < REACT_DELAY:
                sleep(REACT_DELAY - time_spent)
            #if cur_mode == MovementMode.MOVEMENT_STANDBY.value:
            #    loop_count += 1
            #    if loop_count >= standby_loop_count:
            #        loop_count = 0
            #        mode_changed.clear()
            #         print("standby mode over, waiting for mode changed")
        else:
            sleep(1 - REACT_DELAY)